#include "CCar.h"


CCar::CCar(IrrlichtDevice* device, f32 maxSpeed)
{
	this->maxSpeed		= maxSpeed;
	accel				= maxSpeed/100;
	
	linearVelocity		= 0;
	angularVelocity		= 1;
	
	wheelsRotation		= 0;

	wheelsTurn			= 0;
	wheelsMaxTurn		= 40;
	wheelsTurnSpeed		= 2;

	bodySize			= vector3df(0.86f, 0.456f, 2.109f);
	
	wheelRadius			= 0.294f/2.f;

	wheelPosition[0] = vector3df(-0.306f,-0.198f,-0.619f);
	wheelPosition[1] = vector3df( 0.306f,-0.198f,-0.619f);
	wheelPosition[2] = vector3df(-0.327f,-0.198f, 0.538f);
	wheelPosition[3] = vector3df( 0.327f,-0.198f, 0.538f);

	driver = device->getVideoDriver();
	smgr = device->getSceneManager();
		
	collmgr = smgr->getSceneCollisionManager();
	
	// Body
	IMesh* bodyMesh = smgr->getMesh(L"data/models/car/car.x");
	
	if (!bodyMesh) return;

	bodyNode = smgr->addMeshSceneNode(bodyMesh);
	
	if (bodyNode)
	{
		bodyNode->setMaterialTexture(0, driver->getTexture(L"data/textures/car/car_05_clean.tga"));
		bodyNode->setMaterialType(EMT_LIGHTMAP);

		bodyNode->setRotation(vector3df(0.f, 180.f, 0.f));

		bodyNode->setTriangleSelector(smgr->createTriangleSelector(bodyMesh, bodyNode));
	}

	// Wheels
	for (u32 i = 0; i < 4; ++i)
	{
		wheelNode[i] = smgr->addMeshSceneNode(smgr->getMesh(L"data/models/car/wheel.x"));
		
		if (wheelNode[i])
		{
			wheelNode[i]->setPosition(wheelPosition[i]);
			wheelNode[i]->setMaterialTexture(0, driver->getTexture(L"data/textures/car/texture_00001.tga"));
			wheelNode[i]->setMaterialType(EMT_LIGHTMAP);
			wheelNode[i]->setParent(bodyNode);
		}
	}
}


CCar::~CCar()
{
}


void CCar::update(int accelDir, int turnDir)
{
	switch (accelDir)
	{
	case ACCEL_NO:
		{
			if (linearVelocity != 0)
			{
				linearVelocity -= SIGN(linearVelocity) * accel / 16;
				
				if (abs(linearVelocity) <= accel)
				{
					linearVelocity = 0;
				}
			}
			break;
		}
	case ACCEL_FRWD:
		{
			if (linearVelocity > 0)
			{
				if (linearVelocity < maxSpeed)
				{
					linearVelocity += accel;
				}
			}
			else 
			{
				linearVelocity += 2 * accel;
			}
			break;
		}
	case ACCEL_BCKWD:
		{
			if (linearVelocity < 0)
			{
				if (linearVelocity > -maxSpeed/2)
				{
					linearVelocity -= accel;
				}
			}
			else 
			{
				linearVelocity -= 2 * accel;
			}
			break;
		}
	}
	
	switch (turnDir)
	{
	case TURN_NO:
		{
			if (wheelsTurn != 0)
			{
				wheelsTurn -= wheelsTurnSpeed * SIGN(wheelsTurn);
				
				if (abs(wheelsTurn) <= wheelsTurnSpeed)
				{
					wheelsTurn = 0;
				}
			}
			break;
		}
	case TURN_LEFT:
		{
			if (wheelsTurn > -wheelsMaxTurn)
			{
				wheelsTurn -= wheelsTurnSpeed;
			}
			break;
		}
	case TURN_RIGHT:
		{
			if (wheelsTurn < wheelsMaxTurn)
			{
				wheelsTurn += wheelsTurnSpeed;
			}
			break;
		}
	}

	// Save last car position
	lastPosition = bodyNode->getPosition();

	// Update car rotation
	bodyNode->setRotation(bodyNode->getRotation() - 
		vector3df(0.f, angularVelocity * turnDir * SIGN(linearVelocity), 0.f));

	// Update car position
	bodyNode->setPosition(bodyNode->getPosition() - linearVelocity * vector3df(
		sin(bodyNode->getRotation().Y * DEGTORAD), 
		0, 
		cos(bodyNode->getRotation().Y * DEGTORAD)));

	updateWheels();
}


void CCar::updateWheels()
{
	wheelsRotation -= linearVelocity / wheelRadius * RADTODEG;
	
	// Front wheels
	wheelNode[0]->setRotation(vector3df(wheelsRotation, wheelsTurn, 0.f));
	wheelNode[1]->setRotation(vector3df(wheelsRotation, wheelsTurn, 0.f));
	
	// Rear wheels
	wheelNode[2]->setRotation(vector3df(wheelsRotation, 0.f, 0.f));
	wheelNode[3]->setRotation(vector3df(wheelsRotation, 0.f, 0.f));
}


bool CCar::checkCollision(ISceneNode* nodeToCheck)
{
	const ISceneNode* collNode = 0;
	
	vector3df collResultPosition;
	vector3df collPoint;
	triangle3df	collTriangle;
	
	bool f = false;

	f32 deg = (bodyNode->getRotation().Y) * DEGTORAD;

	vector3df radius = vector3df(
		(bodySize.X + abs((bodySize.Z - bodySize.X) * sin(deg)))/2,
		bodySize.Y/2,
		(bodySize.X + abs((bodySize.Z - bodySize.X) * cos(deg)))/2);

	collResultPosition = collmgr->getCollisionResultPosition(
		nodeToCheck->getTriangleSelector(), lastPosition,						
		radius,														// Ellipsoid radius
		bodyNode->getPosition() - lastPosition,						// Ellipsoid direction and speed
		collTriangle, collPoint, f, collNode);

	// If collision occurred
	if (collNode != 0)
	{
		bodyNode->setPosition(
			vector3df(collResultPosition.X, CAR_POS_Y, collResultPosition.Z));
				
		if (linearVelocity > maxSpeed/2)
		{
			bodyNode->setMaterialTexture(0, 
				driver->getTexture(L"data/textures/car/car_05_damaged.tga"));
		}

		// Calculate actual car movement and correct linear velocity
		linearVelocity = (lastPosition - bodyNode->getPosition()).getLength() * SIGN(linearVelocity);

		return true;
	}

	// If no collision
	return false;
}

// Returns turn direction for car movement to target point 
// avoiding collisions with scene objects
int CCar::genTurn(vector3df& point)
{
	// Nodes with which collisions are detected
	ISceneNode* collNodeLeft = 0;
	ISceneNode* collNodeRight = 0;

	// Positions of tne colisions from rays
	vector3df collPointLeft = vector3df();
	vector3df collPointRight = vector3df();
	
	triangle3df triangle = triangle3df();	
		
	f32 rayLenght = 50.0f;
	
	// Ray indent from left and right sides of the car body. From this value 
	// depends how far the car will go around barriers
	f32 rayIndent = 0.2f;

	// Rays with which collisions are tested
	line3df rayLeft;
	line3df	rayRight;

	// Both rays starts from the car sides and cross in front of it
	// Left ray
	rayLeft.start = bodyNode->getPosition() + vector3df(
		(bodySize.X/2 + rayIndent) * sin((bodyNode->getRotation().Y + 90) * DEGTORAD),	
		0, 
		(bodySize.X/2 + rayIndent) * cos((bodyNode->getRotation().Y + 90) * DEGTORAD));

	rayLeft.end = bodyNode->getPosition() - vector3df(
		rayLenght * sin(bodyNode->getRotation().Y * DEGTORAD), 
		0, 
		rayLenght * cos(bodyNode->getRotation().Y * DEGTORAD));

	// Right ray
	rayRight.start = bodyNode->getPosition() + vector3df(
		(bodySize.X/2 + rayIndent) * sin((bodyNode->getRotation().Y - 90) * DEGTORAD),
		0,
		(bodySize.X/2 + rayIndent) * cos((bodyNode->getRotation().Y - 90) * DEGTORAD));

	rayRight.end = bodyNode->getPosition() - vector3df(
		rayLenght * sin(bodyNode->getRotation().Y * DEGTORAD), 
		0, 
		rayLenght * cos(bodyNode->getRotation().Y * DEGTORAD));

	// Check collisions with rays
	collNodeLeft = collmgr->getSceneNodeAndCollisionPointFromRay(rayLeft, collPointLeft, triangle);
	collNodeRight = collmgr->getSceneNodeAndCollisionPointFromRay(rayRight, collPointRight, triangle);
	
	// If both rays are collided
	if ((collNodeLeft != 0) && (collNodeRight != 0))
	{	
		// If both rays are collided with the same object
		if (collNodeLeft == collNodeRight)
		{
			// Choose turn direction depending on the relative position of the barrier and the car
			if(collNodeLeft->getPosition().X >= bodyNode->getPosition().X)
			{
				return TURN_LEFT;
			}
			else
			{
				return TURN_RIGHT;
			}
		}
		// If the objects are different
		else
		{
			rayLeft.end = collPointLeft;		
			rayRight.end = collPointRight;

			// Calculate distance to the collision points and turn in the direction opposite the closer one
			if (rayLeft.getLength() > rayRight.getLength())
			{
				return TURN_LEFT;
			}
			else
			{
				return TURN_RIGHT;
			}
		}
	}
	 
	// In case of one ray collision turn to the side opposite this intersection
	if (collNodeLeft != 0)
	{	
		return TURN_RIGHT;
	}
	if (collNodeRight != 0)
	{	
		return TURN_LEFT;
	}
	
	// Move to target point
	return turnToPoint(point, rayLenght, rayIndent);	
}


// Returns turn dirrection for car movement to the target point
int CCar::turnToPoint(vector3df& point, f32 rayLenght, f32 rayIndent)
{
	// Nodes with which collisions are detected
	ISceneNode* collNodeLeft = 0;
	ISceneNode* collNodeRight = 0;

	// Positions of tne colisions from rays
	vector3df collPointLeft = vector3df();
	vector3df collPointRight = vector3df();
	
	triangle3df triangle = triangle3df();	
	
	// Rays with which collisions are tested
	line3df rayLeft;
	line3df	rayRight;

	// Find the angle pointing to the target
	vector3df toTarget(point - bodyNode->getPosition());
	f32 requiredAngle = toTarget.getHorizontalAngle().Y;

	// Correct angle to make the car moving to the point by the hood (not trunk)
	requiredAngle += 180;

	if (requiredAngle >= 360)
	{
		requiredAngle -= 360;
	}

	// Get current car rotation
	f32 carAngle = bodyNode->getRotation().Y;

	// Normalize angle value to make it comparable with requiredAngle
	while (carAngle >= 360)
	{
		carAngle -= 360;
	}
	while (carAngle < 0)
	{
		carAngle += 360;
	}
	
	// Check if it's needed to correct the course
	if (abs(carAngle - requiredAngle) > angularVelocity)
	{
		// Second pair of reys laid along the car with little outside deviation (2 degrees)
		// and used for detection a moment when car can return to specified course
	
		// Left ray
		rayLeft.start = bodyNode->getPosition() + vector3df(
			(bodySize.X/2 + rayIndent) * sin((carAngle + 90) * DEGTORAD),	
			0, 
			(bodySize.X/2 + rayIndent) * cos((carAngle + 90) * DEGTORAD));

		rayLeft.end = rayLeft.start - vector3df(
			rayLenght * sin((carAngle - 2) * DEGTORAD), 
			0, 
			rayLenght * cos((carAngle - 2) * DEGTORAD));

		// Right ray
		rayRight.start = bodyNode->getPosition() + vector3df(
			(bodySize.X/2 + rayIndent) * sin((carAngle - 90) * DEGTORAD),
			0,
			(bodySize.X/2 + rayIndent) * cos((carAngle - 90) * DEGTORAD));

		rayRight.end = rayRight.start - vector3df(
			rayLenght * sin((carAngle + 2) * DEGTORAD), 
			0, 
			rayLenght * cos((carAngle + 2) * DEGTORAD));

		// Check collisions with rays
		collNodeLeft = collmgr->getSceneNodeAndCollisionPointFromRay(rayLeft, collPointLeft, triangle);
		collNodeRight = collmgr->getSceneNodeAndCollisionPointFromRay(rayRight, collPointRight, triangle);
		
		if (abs(carAngle - requiredAngle) >= 180)
		{
			if ((carAngle > requiredAngle) && (collNodeRight == 0))
			{
				return TURN_RIGHT;
			}
			if ((carAngle < requiredAngle) && (collNodeLeft == 0))
			{
				return TURN_LEFT;
			}
		}
		else
		{
			if ((carAngle > requiredAngle) && (collNodeLeft == 0))
			{
				return TURN_LEFT;
			}
			if ((carAngle < requiredAngle) && (collNodeRight == 0))
			{
				return TURN_RIGHT;
			}
		}
	} // (abs(carAngle - requiredAngle) > angularVelocity)

	// The car has not yet complete a maneuver or already on the right course
	return TURN_NO;
}


ISceneNode* CCar::getNode()
{
	return bodyNode;
}


vector3df CCar::getPosition(void)
{
	return bodyNode->getPosition();
}


f32 CCar::getAngle()
{
	return bodyNode->getRotation().Y;
}


f32 CCar::getSpeed()
{
	return linearVelocity;
}


f32 CCar::getMaxSpeed()
{
	return maxSpeed;
}


void CCar::setMaxSpeed(f32 speed)
{
	maxSpeed = speed;
}


void CCar::setPosition(vector3df& position)
{
	lastPosition = position;

	bodyNode->setPosition(position);
	bodyNode->updateAbsolutePosition();
}